/*
 * @Author: cooper
 * @Date: 2025-03-30 21:35:19
 * @Wechat: cmd_develop
 * @Description: 机械臂驱动
 */
#include "./bsp_arm.h"
#include "string.h" // 用于 memcpy
arm_buffer arm_Buffer;

static SemaphoreHandle_t arm_uart_rx_sem = 0; // 串口接受信号量
static SemaphoreHandle_t arm_uart_tx_sem = 0; // 串口发送信号量
QueueHandle_t arm_message_queue = 0;          // arm消息邮箱,确保消息ok
QueueHandle_t arm_work_queue = 0;             // 长轴工作邮箱

// MKS接收数据处理任务句柄
TaskHandle_t arm_uart_receive_task_handler;
TaskHandle_t arm_work_task_handler;

// 为了方便阅读函数，将基本函数写在文件最后
void ARM_UART_DMA_RX(void);
void ARM_UART_DMA_TX(u16 size);
void DMA1_Channel1_IRQHandler(void);

void arm_uart_receive_task(void *parameter);
void arm_work_task_function(void *parameter);

u8 arm_moto_function(u8 addr, u8 moto_do, u32 pos);

void RX_arm_handle(void);

/*************************************** 4号RS485——arm DMA配置 ***************************************/
/**
 * @brief  arm串口DMA接收
 * @param  none
 * @retval none
 */

void ARM_TASK(void)
{
    // 串口初始化
    ARM_UART_INIT();
    // 初始化信号量
    arm_uart_rx_sem = xSemaphoreCreateBinary();
    arm_uart_tx_sem = xSemaphoreCreateBinary();
    xSemaphoreGive(arm_uart_tx_sem); // 释放发送信号量

    arm_message_queue = xQueueCreate((UBaseType_t)1,  /* 消息队列的长度 */
                                     (UBaseType_t)1); /* 消息的大小 */

    arm_work_queue = xQueueCreate((UBaseType_t)1,  /* 消息队列的长度 */
                                  (UBaseType_t)4); /* 消息的大小 */

    // 创建接收任务
    xTaskCreate((TaskFunction_t)arm_uart_receive_task,
                (const char *)"ARM_RX_TASK",
                (uint16_t)configMINIMAL_STACK_SIZE, // 128
                (void *)NULL,
                (UBaseType_t)4,
                (TaskHandle_t *)&arm_uart_receive_task_handler);
    // 创建长轴工作任务
    xTaskCreate((TaskFunction_t)arm_work_task_function,
                (const char *)"ARM_WORK_TASK",
                (uint16_t)configMINIMAL_STACK_SIZE, // 128
                (void *)NULL,
                (UBaseType_t)3,
                (TaskHandle_t *)&arm_work_task_handler);
}

// 任务函数，等待信号量并处理接收到的数据
void arm_uart_receive_task(void *parameter)
{
    while (1)
    {
        // 等待信号量，直到有数据接收
        if (xSemaphoreTake(arm_uart_rx_sem, portMAX_DELAY) == pdTRUE)
        {
            // logPrintln("arm_RX_FUNCTION:");
            // for (int i = 0; i <= RX_arm_TOTAL - 1; i++)
            // {
            //     logPrintln("i:%d:%x\t", i, arm_Buffer.rx_data[i]);
            // }
            // logPrintln("\n");
            RX_arm_handle();
        }
    }
}

void RX_arm_handle(void)
{
    // 等待信号量，直到 DMA 接收完成
    u8 message_crc8 = RX_arm(RX_arm_TOTAL - 1);
    // logPrintln("arm RX_arm_FUNCTION:");
    // for (int i = 0; i <= RX_arm_TOTAL - 1; i++)
    // {
    //     logPrintln("i:%d:%x\t", i, arm_Buffer.rx_data[i]);
    // }
    // logPrintln("\r\n");
    if (RX_arm(0) == 0xFB)
    {
        if (getCheckSum((&RX_arm(0)), RX_arm_TOTAL - 1) == message_crc8)
        {
            switch (RX_arm(2))
            {
            case 0x32:
            {
            }
            break;
            case 0x31:
            {
            }
            break;
            case 0x91:
            {
                // 归零到位
                if (RX_arm(3) == 0x02)
                {
                    switch (RX_arm(1))
                    {
                    case LONG_ARM_ADDRESS:
                    case SHORT_ARM_ADDRESS:
                    case Z_ARM_ADDRESS:
                    case PAW_ARM_ADDRESS:
                    case STICK_ARM_ADDRESS:
                        TX_arm_BufClear();
                        xQueueSend(arm_message_queue, // 发送队列句柄
                                   &RX_arm(1),        // 发送内容
                                   0);                // 对满不等待
                        break;
                    default:
                        break;
                    }
                }
            }
            break;
            case 0xF5:
            {
                // 移动到位
                if (RX_arm(3) == 0x02)
                {
                    switch (RX_arm(1))
                    {
                    case LONG_ARM_ADDRESS:
                    case SHORT_ARM_ADDRESS:
                    case Z_ARM_ADDRESS:
                    case PAW_ARM_ADDRESS:
                    case STICK_ARM_ADDRESS:
                        TX_arm_BufClear();
                        xQueueSend(arm_message_queue, // 发送队列句柄
                                   &RX_arm(1),        // 发送内容
                                   0);                // 对满不等待
                        break;
                    default:
                        break;
                    }
                }
            }
            break;
            default:
                break;
            }
        }
    }
    ARM_UART_DMA_RX(); // RX准备接受下一次数据
}

/// @brief 机械臂长轴发送命令
/// @param
void arm_cmd_queue_send(u8 code[3])
{
    logPrintln("arm_cmd_queue_send");

    xQueueSend(arm_work_queue, // 发送队列句柄
               code,           // 发送内容
               0);             // 对满不等待
}

// 长轴任务
void arm_work_task_function(void *parameter)
{
    static u8 msg[3] = {0, 0, 0};
    u8 slave = 0;
    u8 pos = 0;
    u8 arm_moto_status = 0;
    ARM_REG[REG_STATUS] = 0;

    logPrintln("arm_work_function\n");
    while (1)
    {
        vTaskDelay(100 / portTICK_PERIOD_MS);
        switch (ARM_REG[REG_STATUS])
        {
        case MOTO_INIT:
            // 将电机初始化，这个暂时省略，先将电机归零
            ARM_REG[REG_STATUS]++;
            // ARM_REG[REG_STATUS] = MOTO_QUEUE; // 测试用
            // do
            // {
            //     arm_moto_status = 0;
            //     arm_moto_status = arm_moto_function(Z_ARM_ADDRESS, MOTO_INIT, 200);
            // } while (!arm_moto_status);
            // do
            // {
            //     arm_moto_status = 0;
            //     arm_moto_status = arm_moto_function(LONG_ARM_ADDRESS, MOTO_INIT, 200);
            // } while (!arm_moto_status);
            // do
            // {
            //     arm_moto_status = 0;
            //     arm_moto_status = arm_moto_function(SHORT_ARM_ADDRESS, MOTO_INIT, 200);
            // } while (!arm_moto_status);
            // do
            // {
            //     arm_moto_status = 0;
            //     arm_moto_status = arm_moto_function(STICK_ARM_ADDRESS, MOTO_INIT, 1000);
            // } while (!arm_moto_status);
            break;
        case MOTO_ZERO:
            // 归零
            do
            {
                arm_moto_status = 0;
                arm_moto_status = arm_moto_function(Z_ARM_ADDRESS, MOTO_ZERO, pos);
            } while (!arm_moto_status);
            do
            {
                arm_moto_status = 0;
                arm_moto_status = arm_moto_function(LONG_ARM_ADDRESS, MOTO_ZERO, pos);
            } while (!arm_moto_status);
            do
            {
                arm_moto_status = 0;
                arm_moto_status = arm_moto_function(SHORT_ARM_ADDRESS, MOTO_ZERO, pos);
            } while (!arm_moto_status);
            do
            {
                arm_moto_status = 0;
                arm_moto_status = arm_moto_function(STICK_ARM_ADDRESS, MOTO_ZERO, pos);
            } while (!arm_moto_status);

            ARM_REG[REG_STATUS] = MOTO_QUEUE;
            break;
        case MOTO_POS:
            // 位移
            arm_moto_status = arm_moto_function(slave, MOTO_POS, pos);
            if (arm_moto_status)
            {
                ARM_REG[REG_STATUS] = MOTO_QUEUE;
            }

            break;
        case MOTO_QUEUE:
            // 阻塞
            logPrintln("ARM MOTO_QUEUE\n");
            if (xQueueReceive(arm_work_queue, &msg, portMAX_DELAY) == pdTRUE)
            {
                logPrintln("arm_work_queue,MOTO_QUEUE: %d %d %d", msg[0], msg[1], msg[2]);
                pos = msg[1];
                slave = msg[2];
                ARM_REG[REG_STATUS] = msg[0]; // 进入对应的操作码
            }
            break;
        default:
            break;
        }
    }
}

/// @brief
/// @param addr 从机地址
/// @param moto_do 电机去干什么：归零、位置移动
/// @param pos 位置参数
/// @return
u8 arm_moto_function(u8 addr, u8 moto_do, u32 pos)
{
    u8 queue_code = 0;
    u16 speed;
    u32 *error_times = NULL;
    u8 acc;
    u32 relAxis;
    switch (moto_do)
    {
    case MOTO_INIT:
        logPrintln("arm_moto_function,TX MOTO_ZERO\n");
        TX_arm_TOTAL = MKS_Set_GoHone_retValue_function(&(TX_arm(0)), addr, pos);
        logPrintln(" TX_arm_TOTAL= %d\n", TX_arm_TOTAL);
        for (int i = 0; i <= TX_arm_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, arm_Buffer.tx_data[i]);
        }
        ARM_UART_DMA_TX(TX_arm_TOTAL);

        //   等待邮箱信息
        if (xQueueReceive(arm_message_queue, &queue_code, WAITTIME) == pdTRUE)
        {
            // 归零成功
            logPrintln("init success\n");
            return 1;
        }
        else
        {
            // 超时,报警
            logPrintln("init overtime\n");
        }
        break;
    case MOTO_ZERO:
        // 归零
        logPrintln("arm_moto_function,TX MOTO_ZERO\n");
        TX_arm_TOTAL = moto_run_home_function(&(TX_arm(0)), addr);
        logPrintln(" TX_arm_TOTAL= %d\n", TX_arm_TOTAL);
        for (int i = 0; i <= TX_arm_TOTAL - 1; i++)
        {
            logPrintln("i:%d:%x\t", i, arm_Buffer.tx_data[i]);
        }
        ARM_UART_DMA_TX(TX_arm_TOTAL);

        //   等待邮箱信息
        if (xQueueReceive(arm_message_queue, &queue_code, WAITTIME) == pdTRUE)
        {
            // 归零成功
            logPrintln("arm zero success\n");
            return 1;
        }
        else
        {
            // 超时,报警
            logPrintln("arm zero overtime\n");
        }
        break;
    case MOTO_POS:
        if (addr == LONG_ARM_ADDRESS)
        {
            speed = LONG_ARM_REG[REG_SPEED];
            acc = LONG_ARM_REG[REG_ACC];
            relAxis = LONG_ARM_REG[pos];
            error_times = &LONG_ARM_REG[REG_ERROR_TIMES];
        }
        else if (addr == SHORT_ARM_ADDRESS)
        {
            speed = SHORT_ARM_REG[REG_SPEED];
            acc = SHORT_ARM_REG[REG_ACC];
            relAxis = SHORT_ARM_REG[pos];
            error_times = &SHORT_ARM_REG[REG_ERROR_TIMES];
        }
        else if (addr == Z_ARM_ADDRESS)
        {
            speed = Z_ARM_REG[REG_SPEED];
            acc = Z_ARM_REG[REG_ACC];
            relAxis = Z_ARM_REG[pos];
            error_times = &Z_ARM_REG[REG_ERROR_TIMES];
        }
        else if (addr == PAW_ARM_ADDRESS)
        {
            speed = PAW_ARM_REG[REG_SPEED];
            acc = PAW_ARM_REG[REG_ACC];
            relAxis = PAW_ARM_REG[pos];
            error_times = &PAW_ARM_REG[REG_ERROR_TIMES];
        }
        else if (addr == STICK_ARM_ADDRESS)
        {
            speed = STICK_ARM_REG[REG_SPEED];
            acc = STICK_ARM_REG[REG_ACC];
            relAxis = STICK_ARM_REG[pos];
            error_times = &STICK_ARM_REG[REG_ERROR_TIMES];
        }
        if (xSemaphoreTake(arm_uart_tx_sem, portMAX_DELAY) == pdTRUE)
        {
            TX_arm_TOTAL = moto_run_rel_function(&(TX_arm(0)), addr, speed, acc, relAxis);
            logPrintln(" TX_arm_TOTAL= %d\n", TX_arm_TOTAL);
            for (int i = 0; i <= TX_arm_TOTAL - 1; i++)
            {
                logPrintln("i:%d:%x\t", i, arm_Buffer.tx_data[i]);
            }
            ARM_UART_DMA_TX(TX_arm_TOTAL);
        }

        //   等待队列信息
        if (xQueueReceive(arm_message_queue, &queue_code, WAITTIME) == pdTRUE)
        {
            // 移动成功
            logPrintln("move success\n");
            *error_times = 0;
            xSemaphoreGive(arm_uart_tx_sem); // 释放发送信号量
            xQueueSend(step_work_queue,      // 发送队列句柄
                       &moto_do,             // 发送内容
                       0);                   // 对满不等待
            return 1;
        }
        else
        {
            // 超时,报警
            *error_times++;
            logPrintln("移动超时 %d\n", *error_times);

            xSemaphoreGive(arm_uart_tx_sem); // 释放发送信号量
        }
        break;
    default:
        break;
    }
    return 0;
}

/// @brief arm串口DMA接受
/// @param
void ARM_UART_DMA_RX(void)
{
    dma_init_type dma_init_struct;
    /* 使能时钟 */
    crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE);
    /* 使能dma复用功能 */
    dmamux_enable(DMA1, TRUE);
    /* 复位指定的 DMA 通道 */
    dma_reset(DMA1_CHANNEL1);
    /* 默认填充配置 */
    dma_default_para_init(&dma_init_struct);
    /* DMA 传输数据量+传输方向 */
    dma_init_struct.buffer_size = arm_DATA_SIZE;
    dma_init_struct.direction = DMA_DIR_PERIPHERAL_TO_MEMORY;
    /* 外设地址+数据宽度+地址增加模式 */
    dma_init_struct.peripheral_base_addr = (uint32_t)&UART8->dt;
    dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE;
    dma_init_struct.peripheral_inc_enable = FALSE;
    /* 存储器地址+数据宽度+地址增加模式 */
    dma_init_struct.memory_base_addr = (uint32_t)(&(RX_arm(0)));
    dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE;
    dma_init_struct.memory_inc_enable = TRUE;
    /* 优先级+循环模式 */
    dma_init_struct.priority = DMA_PRIORITY_HIGH;
    dma_init_struct.loop_mode_enable = FALSE;
    /* 初始化指定的 DMA 通道 */
    dma_init(DMA1_CHANNEL1, &dma_init_struct);
    /* 初始化dma复用功能 */
    dmamux_init(DMA1MUX_CHANNEL1, DMAMUX_DMAREQ_ID_UART8_RX);
    /* 使能指定通道 */
    dma_channel_enable(DMA1_CHANNEL1, TRUE);
    /* 数据接收区数据清理 */
    RX_arm_BufClear();
}

/**
 * @brief  arm串口DMA发送
 * @param  none
 * @retval none
 */
void ARM_UART_DMA_TX(u16 size)
{
    dma_init_type dma_init_struct;
    /* 使能时钟 */
    crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE);
    /* 使能dma复用功能 */
    dmamux_enable(DMA1, TRUE);
    /* 复位指定的 DMA 通道 */
    dma_reset(DMA1_CHANNEL1);
    /* 默认填充配置 */
    dma_default_para_init(&dma_init_struct);
    /* DMA 传输数据量+传输方向 */
    dma_init_struct.buffer_size = size;
    dma_init_struct.direction = DMA_DIR_MEMORY_TO_PERIPHERAL;
    /* 存储器地址+数据宽度+地址增加模式 */
    dma_init_struct.memory_base_addr = (uint32_t)(&(TX_arm(0)));
    dma_init_struct.memory_data_width = DMA_MEMORY_DATA_WIDTH_BYTE;
    dma_init_struct.memory_inc_enable = TRUE;
    /* 外设地址+数据宽度+地址增加模式 */
    dma_init_struct.peripheral_base_addr = (uint32_t)&UART8->dt;
    dma_init_struct.peripheral_data_width = DMA_PERIPHERAL_DATA_WIDTH_BYTE;
    dma_init_struct.peripheral_inc_enable = FALSE;
    /* 优先级+循环模式 */
    dma_init_struct.priority = DMA_PRIORITY_HIGH;
    dma_init_struct.loop_mode_enable = FALSE;
    /* 使能DMA发送完成中断 */
    dma_interrupt_enable(DMA1_CHANNEL1, DMA_FDT_INT, TRUE);
    nvic_irq_enable(DMA1_Channel1_IRQn, 2, 2);
    /* 初始化指定的 DMA 通道 */
    dma_init(DMA1_CHANNEL1, &dma_init_struct);
    /* 初始化dma复用功能 */
    dmamux_init(DMA1MUX_CHANNEL1, DMAMUX_DMAREQ_ID_UART8_TX);
    /* 使能指定通道 */
    dma_channel_enable(DMA1_CHANNEL1, TRUE);
}

/**
 * @brief  4号RS485——arm初始化
 * @param  none
 * @retval none
 */
void ARM_UART_INIT(void)
{
    gpio_init_type gpio_init_struct;
    /* 使能时钟 */
    crm_periph_clock_enable(CRM_UART8_PERIPH_CLOCK, TRUE);
    crm_periph_clock_enable(CRM_GPIOD_PERIPH_CLOCK, TRUE);
    /* GPIO配置 */
    gpio_default_para_init(&gpio_init_struct);
    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_pins = GPIO_PINS_0 | GPIO_PINS_1;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
    gpio_init(GPIOE, &gpio_init_struct);
    /* 复用 */
    gpio_pin_mux_config(GPIOE, GPIO_PINS_SOURCE1, GPIO_MUX_8);
    gpio_pin_mux_config(GPIOE, GPIO_PINS_SOURCE0, GPIO_MUX_8);
    /* 串口参数 */
    usart_reset(UART8);
    usart_init(UART8, 115200, USART_DATA_8BITS, USART_STOP_1_BIT);
    usart_transmitter_enable(UART8, TRUE);
    usart_receiver_enable(UART8, TRUE);
    /* 中断配置 */
    nvic_irq_enable(UART8_IRQn, 2, 2);
    usart_interrupt_enable(UART8, USART_IDLE_INT, TRUE);
    /* 使用DMA */
    usart_dma_receiver_enable(UART8, TRUE);
    usart_dma_transmitter_enable(UART8, TRUE);
    /* 使能 */
    usart_enable(UART8, TRUE);
    /* DMA接收 */
    ARM_UART_DMA_RX();
}

/**
 * @brief  arm串口3中断处理函数
 * @param  none
 * @retval none
 */
void UART8_IRQHandler(void)
{
    uint32_t ulReturn;
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    /*进入临界段*/
    ulReturn = taskENTER_CRITICAL_FROM_ISR();
    if (usart_flag_get(UART8, USART_IDLEF_FLAG) != RESET)
    {
        dma_channel_enable(DMA1_CHANNEL1, FALSE);
        dma_flag_clear(DMA1_FDT1_FLAG);
        // 获取接收总数
        RX_arm_TOTAL = arm_DATA_SIZE - dma_data_number_get(DMA1_CHANNEL1);
        // 释放信号量
        xSemaphoreGiveFromISR(arm_uart_rx_sem, &xHigherPriorityTaskWoken);
        // 切换任务
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
        usart_flag_clear(UART8, USART_IDLEF_FLAG);
    }
    /* 推出临界段 */
    taskEXIT_CRITICAL_FROM_ISR(ulReturn);
}

/**
 * @brief  arm串口DMA1通道1中断处理函数
 * @param  none
 * @retval none
 */
void DMA1_Channel1_IRQHandler(void)
{
    uint32_t ulReturn;
    /* 进入临界段，临界段可以嵌套 */
    ulReturn = taskENTER_CRITICAL_FROM_ISR();
    if (dma_interrupt_flag_get(DMA1_FDT1_FLAG))
    {
        // DMA接收
        ARM_UART_DMA_RX();
        dma_flag_clear(DMA1_FDT1_FLAG);
    }
    /* 退出临界段 */
    taskEXIT_CRITICAL_FROM_ISR(ulReturn);
}
